/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/Src/side_path.cpp
 * @Description  : 实现了SidePath类，提供了初始化侧移径对象、计算路径长度、生成轨迹对象等功能。 
 *                 包括获取车辆在指定节点的航向角，计算路径的尺寸，并创建侧移路径轨迹。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:31:57
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdlib.h> 
#include <stdio.h>
#include "path.h"
#include "tools.h"

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CSidePath".
//
//   GetHeading: Get the vehicle's heading angle at the specified node.
//
Angle& SidePath::GetHeading(Node node/*node*/) 
{
    // The vehicle's heading is the same everywhere on the path
    return heading_angle_;
}

/**
 * @brief 初始化SidePath对象
 *
 * 该函数用于初始化SidePath对象，包括设置航向角和计算尺寸。
 */
void SidePath::Init() 
{
    heading_angle_ = Angle(ang_head_);
    size_ = CalculateSize();
}

/**
 * @brief 计算路径长度
 *
 * 根据路径的起点和终点，计算路径的长度。
 *
 * @return 返回路径的长度，类型为 float
 */
float SidePath::CalculateSize() const 
{
    Point start_point = GetStartPoint();
    Point end_point = GetEndPoint();
    Line line(start_point, end_point);
    return line.Length();
}

/**
 * @brief 生成一个轨迹对象
 *
 * 根据起始点和终止点生成一个侧路径轨迹对象，并设置航向角
 *
 * @return 返回生成的轨迹对象指针
 */
Trajectory* SidePath::MakeTraj() 
{
    auto* side_traj = new SideTrajectory;
    Point start_point = GetStartPoint();
    Point end_point = GetEndPoint();
    side_traj->CreateTrajectory(start_point, end_point, heading_angle_);
    return side_traj;
}

/**
 * @brief 创建SidePath对象
 *
 * 初始化SidePath对象并返回创建是否成功。
 *
 * @return 如果创建成功返回true，否则返回false
 */
bool SidePath::Create() 
{
    Init();
    return true;
}